#include "vectors.h"
#define MATRIX_NUM 4

template<class T>
class Matrix
{
public:

	// Constructors & Destructor
	Matrix(){ SetZero(); }
	virtual ~Matrix(){}

	// 数据访问
	T Get(int i, int j) const;
	void Set(int i, int j, T value);

	// 特殊矩阵
	void SetZero();									// 零矩阵
	void SetIdentity();								// 单位矩阵

	// overload operators
	Matrix& operator=(const Matrix& rhs);
	Matrix& operator+=(const Matrix& rhs);
	Matrix& operator-=(const Matrix& rhs);
	Matrix& operator*=(const T& rhs);
	Matrix& operator*=(const Matrix& rhs);
	const Matrix operator*(const Matrix& rhs) const;
	const Matrix operator*(const T& rhs) const;
	const Matrix operator/(const T& rhs) const;
	const Matrix operator-(const Matrix& rhs) const;
	const Matrix operator+(const Matrix& rhs) const;
	bool operator==(const Matrix& rhs) const;
	bool operator!=(const Matrix& rhs) const;
	friend Matrix operator*(const T& lhs, const Matrix& rhs);

	// 转化
	void Translate(T x, T y, T z);			// 平移
	void Scale(T x, T y, T z);							// 缩放
	void Scale(T s) { Scale(s, s, s); }
	void XRotate(float angel);							// 绕X轴旋转
	void YRotate(float angel);							// 绕Y轴旋转
	void ZRotate(float angel);							// 绕Z轴旋转
	// void Transform(Vec4f &v);
	// void Transform(Vec3f &v);
	// void Transform(Vec2f &v);
protected:
	bool IsCoordinatesCurrent(int i, int y) const;		// 检验坐标是不是合法

protected:
	T _data[MATRIX_NUM][MATRIX_NUM];					// 私有成员，矩阵数据

	friend class Vec4f;
};

template<class T>
T Matrix<T>::Get(int i, int j) const
{
	assert(IsCoordinatesCurrent(i, j));
	return _data[i][j];
}


template<class T>
void Matrix<T>::Set(int i, int j, T value)
{
	assert(IsCoordinatesCurrent(i, j));
	_data[i][j] = value;
}

template<class T>
void Matrix<T>::SetZero()
{
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			_data[i][j] = static_cast<T>(0);
		}
	}
}

template<class T>
void Matrix<T>::SetIdentity()
{
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			_data[i][j] = static_cast<T>(i == j);
		}
	}
}

template<class T>
bool Matrix<T>::IsCoordinatesCurrent(int i, int j) const
{
	if((i < MATRIX_NUM && i >= 0) || (j < MATRIX_NUM && j >= 0))
		return true;
	else
		return false;
}

template<class T>
Matrix<T>& Matrix<T>::operator=(const Matrix& rhs)
{
	if(this == &rhs) return *this;

	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			(*this)._data[i][j] = rhs._data[i][j];
		}
	}

	return *this;
}

template<class T>
Matrix<T>& Matrix<T>::operator+=(const Matrix& rhs)
{
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			_data[i][j] += rhs._data[i][j];
		}
	}
	return *this;
}

template<class T>
Matrix<T>& Matrix<T>::operator-=(const Matrix& rhs)
{
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			_data[i][j] -= rhs._data[i][j];
		}
	}
	return *this;
}

template<class T>
Matrix<T>& Matrix<T>::operator*=(const T& rhs)
{
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			_data[i][j] = rhs * _data[i][j];
		}
	}
	return *this;
}

template<class T>
Matrix<T>& Matrix<T>::operator*=(const Matrix& rhs)
{
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			_data[i][j] = _data[i][0] * rhs._data[0][j];
			for(int k = 1; k < MATRIX_NUM; k++)
			{
				_data[i][j] += _data[i][k] * rhs._data[k][j];
			}
		}
	}
	return *this;
}

template<class T>
const Matrix<T> Matrix<T>::operator*(const Matrix& rhs) const
{
	Matrix<T> result;
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			for(int k = 0; k < MATRIX_NUM; k++)
			{
				result._data[i][j] += _data[i][k] * rhs._data[k][j];
			}
		}
	}
	return result;
}

template<class T>
const Matrix<T> Matrix<T>::operator*(const T& rhs) const
{
	Matrix<T> result;
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			result._data[i][j] = _data[i][j] * rhs;
		}
	}
	return result;
}

template<class T>
const Matrix<T> Matrix<T>::operator/(const T& rhs) const
{
	Matrix<T> result;
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			result._data[i][j] = _data[i][j] / rhs;
		}
	}
	return result;
}

template<class T>
const Matrix<T> Matrix<T>::operator-(const Matrix& rhs) const
{
	Matrix result;
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			result._data[i][j] = _data[i][j] - rhs._data[i][j];
		}
	}
	return result;
}

template<class T>
const Matrix<T> Matrix<T>::operator+(const Matrix& rhs) const
{
	Matrix result;
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			result._data[i][j] = _data[i][j] + rhs._data[i][j];
		}
	}
	return result;

}

template<class T>
bool Matrix<T>::operator==(const Matrix& rhs) const
{
	for(int i = 0; i < MATRIX_NUM; i++)
	{
		for(int j = 0; j < MATRIX_NUM; j++)
		{
			if (_data[i][j] != rhs._data[i][j]) return false;
		}
	}
	return true;
}

template<class T>
bool Matrix<T>::operator!=(const Matrix& rhs) const
{
	return !((*this) == rhs);
}

template<class T>
Matrix<T> operator*(const T& lhs, const Matrix<T>& rhs)
{
	return rhs * lhs;
}

template<class T>
void Matrix<T>::Translate(T x, T y, T z)
{
	Matrix<T> t;
	t.SetIdentity();
	t._data[0][3] = x;
	t._data[1][3] = y;
	t._data[2][3] = z;

	*this = t * (*this);		// 不可写做(*this) * t，顺序在矩阵乘法中很重要
}

template<class T>
void Matrix<T>::Scale(T x, T y, T z)
{
	Matrix<T> t;
	t.SetIdentity();
	t._data[0][0] = x;
	t._data[1][1] = y;
	t._data[2][2] = z;

	*this = t * (*this);
}

template<class T>
void Matrix<T>::XRotate(float angel)
{
	Matrix<T> rx;
	rx.SetIdentity();
	rx._data[1][1] = static_cast<T>(cos(angel));
	rx._data[1][2] = static_cast<T>(-sin(angel));
	rx._data[2][1] = static_cast<T>(sin(angel));
	rx._data[2][2] = static_cast<T>(cos(angel));
	*this = rx * (*this);
}

template<class T>
void Matrix<T>::YRotate(float angel)
{
	Matrix<T> ry;
	ry.SetToIdentity();
	ry._data[0][0]= static_cast<T>(cos(angel));
	ry._data[0][2]= static_cast<T>(sin(angel));
	ry._data[2][0]= static_cast<T>(-sin(angel));
	ry._data[2][2]= static_cast<T>(cos(angel));
	*this = ry * (*this);
}

template<class T>
void Matrix<T>::ZRotate(float angel)
{
	Matrix<T> rz;
	rz.SetToIdentity();
	rz._data[0][0]= static_cast<T>(cos(angel));
	rz._data[0][1]= static_cast<T>(-sin(angel));
	rz._data[1][0]= static_cast<T>(sin(angel));
	rz._data[1][1]= static_cast<T>(cos(angel));
	*this = rz * (*this);
}